////
////  test Lie.cpp
////  markerARLib
////
////  Created by fg on 5/14/16.
////  Copyright © 2016 CvidKal. All rights reserved.
////
//
//#include <stdio.h>
//
//
//
//#include "PoseOptimiser.hpp"
//
//#include <sophus/se3.h>
//#include <sophus/so3.h>
//using namespace PoseOptimiser;
//using namespace Sophus;
//
//
//int main()
//{
//    std::vector<Feature*> fts;
//    std::srand(std::time(0)); //use current time as seed for random generator
//
//    
//    Eigen::Matrix3d k;
//    double fx=420;
//    k(0,0) = k(1,1) = fx;
//    k(0,2) = 640/2;
//    k(1,2) = 480/2;
//    IntrinsicMatrix K(k);
//    Frame frame;
//    frame.K = &K;
//    Pose p1;
//    p1.R.setIdentity();
//    p1.t.setZero();
//    p1.t[2] = 0.2;
//    frame.C = p1;
//    SE3 pose(p1.R,p1.t);
//    auto invPose = pose.inverse();
//    double depth = 500;
//    for (int i=0; i<10; i++) {
//        float x = (rand()/(float)RAND_MAX)*640;
//        float y =(rand()/(float)RAND_MAX)*480;
//        
//        Feature * f = new Feature(x,y,&frame);
////        float dx =(rand()/(float)RAND_MAX)*2;
////        float dy =(rand()/(float)RAND_MAX)*2;
////        f->_pos[0] =400;
////        f->_pos[1] =200;
//        frame.fts.push_back(f);
//        auto worldP = invPose*f->f*depth;
//
//        MapPoint * mp = new MapPoint(worldP);
//        f->pointXYZ = mp;
//    }
//    frame.C.t.z() = 0;
////    testJacobian(&frame);
//    PoseOptimiseLM(&frame);
////
////    Eigen::Matrix3d R;
////    double theta = M_PI/3;
////    R(0,0) = cos(theta);
////    R(0,1) = sin(theta);
////    R(1,0) = -cos(theta);
////    R(1,1) = cos(theta);
////    Eigen::Vector3d t2;
////    t2[2] = 5;
////    SE3 pose2(R,t2);
////    Eigen::Vector3d p2;
////    p2 = pose2*worldP;
////    auto i2 = K.project(p2);
////    cout<<i2<<endl;
////    
////    
//    
//    
//}